#include "rtk_planning_node.h"
#include "rtk_data_path.h"
#include "rtk_planning_config.pb.h"
#include "common_tool/proto_file.h"
RtkPlanningNode::RtkPlanningNode(const std::string filename, double loop_rate = 10) : rtk_planning_core_(filename), loop_rate_(loop_rate)
{
	planing_pub_ = nh_.advertise<auto_msgs::planning>("planning_path", 5);
}

void RtkPlanningNode::LoopProc(const std::vector<double> current_position2d)
{

	rtk_planning_core_.searchNearstPoint(current_position2d);
	rtk_planning_core_.pathBePublished(100, 11);
	planing_pub_.publish(rtk_planning_core_.add_csv_ros_path_.path_);
}

void RtkPlanningNode::LoopProc()

{
	LoopProc(current_position2d_);
}
void RtkPlanningNode::CBLocalizition(const auto_msgs::localizition &msg)
{
	current_position2d_[0] = msg.x;
	current_position2d_[1] = msg.y;
	ROS_INFO("current pose is %f,%f", current_position2d_[0], current_position2d_[1]);
}
int main(int argc, char **argv)
{
	rtk_planning::rtk_planning_config rtk_config;
	if (!common_tool::GetProtoFromASCIIFile(rtk_config_path + "rtk_planning_config.proto", &rtk_config))
	{
		std::cout << "error" << std::endl;
	}

	std::string rtk_data_filepath = rtk_data_path + rtk_config.rtk_record_filename();

	ros::init(argc, argv, "rtk_planning_node");
	ROS_INFO("INIT SUSCESS");
	RtkPlanningNode rtk_planning_node(rtk_data_filepath, 20.0);

	ROS_INFO("NODE INIT");
	std::vector<double> temp{0, 0};
	while (1)
	{
		rtk_planning_node.LoopProc();
		ros::spinOnce(); //可用回调函数
		rtk_planning_node.loop_rate_.sleep();
	}

	return 0;
}